#include "joints.h"
#include <QDebug>

//**************************
//	GENERIC JOINT
//**************************

Joint::Joint(int type)
{
	jointType = type;
	
}

Joint::Joint(int type, float transK, float rotK, float transI, float rotI)
{
	jointType = type;
	translationAlongK = transK;
	rotationAroundK = rotK;
	translationAlongI = transI;
	rotationAroundI = rotI;
	
	drawAxes = false;
}

void Joint::setCenter(coord3D position)
{
    centerPosition.setValues(position.x(), position.y(), position.z());
	//this fuction should only ever be used by the core to set the calculated joint center position
}

void Joint::setStaticCenter(coord3D position)
{
    staticCenter.setValues(position.x(),position.y(),position.z());
}

void Joint::setOrientationK(vector3D orientation)
{
	orientationOfK = vector3D(orientation.x(), orientation.y(), orientation.z());
}

void Joint::setStaticOrientationK(vector3D orientation)
{
        staticOrientationOFK = vector3D(orientation.x(), orientation.y(), orientation.z());
}

bool Joint::axesDrawn()
{
    return drawAxes;
}

coord3D Joint::getCenter()
{
	return centerPosition;
}

coord3D Joint::getStaticCenter()
{
    return staticCenter;
}

vector3D Joint::orientationK()
{
	return orientationOfK;
}

vector3D Joint::staticOrientationK()
{
        return staticOrientationOFK;
}

vector3D Joint::orientationI()
{
	return orientationOfI;
}

float Joint::dhTranslationK()
{
	return translationAlongK;
}

float Joint::dhRotationK()
{
	return rotationAroundK;
}

float Joint::dhTranslationI()
{
	return translationAlongI;
}

float Joint::dhRotationI()
{
	return rotationAroundI;
}

void Joint::setTranslationK(float distance)
{
	translationAlongK = distance;
}

void Joint::setRotationK(float angle)
{
	rotationAroundK = angle;
}

void Joint::setTranslationI(float distance)
{
	translationAlongI = distance;
}

void Joint::setRotationI(float angle)
{
	rotationAroundI = angle;
}

void Joint::actuate(float actuateLevel)	//takes the full range -- so degrees and meters
{
	switch (jointType)
	{
		case REVOLUTE:
			currentRotationAngle = actuateLevel;
	
			if (currentRotationAngle > rightStopAngle) 
				currentRotationAngle = rightStopAngle;
			if (currentRotationAngle < leftStopAngle)
				currentRotationAngle = leftStopAngle;
			break;
			
		case PRISMATIC:
                        currentExtension = actuateLevel;
	
			if (currentExtension < minExtension)
				currentExtension = minExtension;
			if (currentExtension > maxExtension)
				currentExtension = maxExtension;
			break;	
			
		case GRIPPER:
			if (actuateLevel > 0.0)
				clenched = true;
			else
				clenched = false;
			break;
			
		case WELDER:
			if (actuateLevel > 0.0)
				welding = true;
			else
				welding = false;
			break;
	}
	
}

void Joint::setDrawAxes(bool draw)
{
    if (draw == false)
    {
        drawAxes = false;
    }
    else
    {
        drawAxes = true;
    }
}

int Joint::getType()
{
	return jointType;
}

float Joint::getMinimum()
{
    switch (jointType)
    {
	case REVOLUTE:
	    return leftStopAngle;
	case PRISMATIC:
	    return minExtension;
	case GRIPPER:
	    return 0.0f;
	case WELDER:
	    return 0.0f;
    }
}

float Joint::getMaximum()
{
    switch (jointType)
    {
	case REVOLUTE:
	    return rightStopAngle;
	case PRISMATIC:
	    return maxExtension;
	case GRIPPER:
	    return 1.0f;
	case WELDER:
	    return 1.0f;
    }
}

float Joint::getCurrentActuation()
{
    switch (jointType)
    {
	case REVOLUTE:
	    return currentRotationAngle;
	case PRISMATIC:
	    return currentExtension;
	case GRIPPER:
	    return 0.0f;
	case WELDER:
	    return 0.0f;
    }
}

float Joint::getRotation(bool *ok)
{
	if (jointType != REVOLUTE)
	{
		*ok = false;
		return 0.0;
	}
	else
	{
		*ok = true;	
		return currentRotationAngle;
	}
}

bool Joint::rotate(float angle)
{
	if (jointType != REVOLUTE)
		return false;
		
	currentRotationAngle = angle;
	
	if (currentRotationAngle > rightStopAngle) 
		currentRotationAngle = rightStopAngle;
	if (currentRotationAngle < leftStopAngle)
		currentRotationAngle = leftStopAngle;
	
	return true;
}


float Joint::getExtension(bool *ok)
{
	if (jointType != PRISMATIC)
	{
		*ok = false;
		return 0.0;
	}
	else
	{
		*ok = true;
		return currentExtension;
	}
}

bool Joint::extend(float extension)
{
	if (jointType != PRISMATIC)
		return false;
	
	currentExtension = extension;
	
	if (currentExtension < minExtension)
		currentExtension = minExtension;
	if (currentExtension > maxExtension)
		currentExtension = maxExtension;
	
	return true;
}

bool Joint::getGripperStatus(bool *ok)
{
	if (jointType != GRIPPER)
	{
		*ok = false;
		return false;
	}
	else
	{
		*ok = true;
		return clenched;
	}
}

bool Joint::gripClench()
{
	if (jointType != GRIPPER)
		return false;
	else
	{
		clenched = true;
		return true;
	}
}

bool Joint::gripRelease()
{
	if (jointType != GRIPPER)
		return false;
	else
	{
		clenched = false;
		return true;
	}
}

bool Joint::setRotationLimits(float min, float max)
{
	if (jointType != REVOLUTE)
		return false;
	else
	{
		leftStopAngle = min;
		rightStopAngle = max;
		return true;
	}
}

bool Joint::setExtensionLimits(float min, float max)
{
	if (jointType != PRISMATIC)
		return false;
	else
	{
		minExtension = min;
		maxExtension = max;
		return true;
	}
}
